module Timer1PWMP {
	provides {
		interface MotorControl as Motor1;
		interface MotorControl as Motor2;
		interface MotorControl as Motor3;
		interface Init;
		interface McuPowerOverride;
	}
	
	uses {
    	interface HplAtm128TimerCtrl16 as TimerCtrl;

		interface HplAtm128Compare<uint16_t> as CompareA;
		interface HplAtm128Compare<uint16_t> as CompareB;
		interface HplAtm128Compare<uint16_t> as CompareC;
		interface GeneralIO as DirAPin;
		interface GeneralIO as DirBPin;
		interface GeneralIO as DirCPin;
	}
}

implementation {
	Atm128_TCCR1A_t tccr1a;
	Atm128_TCCR1B_t tccr1b;
	
	command error_t Init.init() {
		/*
		tccr1a.flat = 0;
		tccr1a.bits.wgm10 = 3; //(ATM128_WAVE16_PWM_10BIT & 3);

		tccr1a.bits.comC = 0;
		tccr1a.bits.comB = 0;
		tccr1a.bits.comA = 2;
		call TimerCtrl.setCtrlCompare( tccr1a );


		tccr1b.flat = 0;
		tccr1b.bits.cs = 2; // CK/8
		tccr1b.bits.wgm32 = 0; //(ATM128_WAVE16_PWM_10BIT >> 2);
		call TimerCtrl.setCtrlCapture( tccr1b );
		*/
		SET_BIT(TCCR1A, COM1A1); // COM1A1 = 1
		CLR_BIT(TCCR1A, COM1A0); // COM1A0 = 0

		SET_BIT(TCCR1A, COM1B1); // COM1B1 = 1
		CLR_BIT(TCCR1A, COM1B0); // COM1B0 = 0

		SET_BIT(TCCR1A, COM1C1); // COM1C1 = 1
		CLR_BIT(TCCR1A, COM1C0); // COM1C0 = 0

		CLR_BIT(TCCR1B, WGM13);  // Phase correct, 10 bit
		CLR_BIT(TCCR1B, WGM12); 
		SET_BIT(TCCR1A, WGM11);
		SET_BIT(TCCR1A, WGM10);
		
		CLR_BIT(TCCR1B, CS12);   // Set prescaler to CK/8
		SET_BIT(TCCR1B, CS11);
		CLR_BIT(TCCR1B, CS10);

		OCR1A=0;
		OCR1B=0;
		OCR1C=0;
		
		SET_BIT(DDRB, 5);
		SET_BIT(DDRB, 6);
		SET_BIT(DDRB, 7);
		CLR_BIT(PORTB, 5);
		CLR_BIT(PORTB, 6);
		CLR_BIT(PORTB, 7);

		call DirAPin.clr();
		call DirAPin.makeOutput();

		call DirBPin.clr();
		call DirBPin.makeOutput();

		call DirCPin.clr();
		call DirCPin.makeOutput();
		return SUCCESS;
	}

	command error_t Motor1.setSpeed(int16_t speed) {

		if (speed >= 0) {
			call CompareA.set(speed>>5);
			call DirAPin.clr();
		} else  {
			call CompareA.set(speed>>5);
			call DirAPin.set();
		}
		return SUCCESS;
	}

	command error_t Motor2.setSpeed(int16_t speed) {
		if (speed >=0 ) {
			call DirBPin.clr();
			call CompareB.set(speed>>5);
		} else  {
			call DirBPin.set();
			call CompareB.set(speed>>5);
		}
		return SUCCESS;
	}

	command error_t Motor3.setSpeed(int16_t speed) {
		if (speed >=0 ) {
			call DirCPin.clr();
			call CompareC.set(speed>>5);
		} else  {
			call DirCPin.set();
			call CompareC.set(speed>>5);
		}
		return SUCCESS;
	}
	async event void CompareA.fired() {}
	async event void CompareB.fired() {}
	async event void CompareC.fired() {}
	
	async command mcu_power_t McuPowerOverride.lowestState() {
		return ATM128_POWER_IDLE;
	}
}
